﻿
namespace TrackSystem.IO.Formula
{
    /// <summary>
    /// 经纬计算函数
    /// 
    /// </summary>
    public class LonLatTools
    {
        /// <summary>
        /// 计算两个经纬度之间的距离，单位是m
        /// </summary>
        /// <param name="srcLon"></param>
        /// <param name="srcLat"></param>
        /// <param name="dstLon"></param>
        /// <param name="dstLat"></param>
        /// <returns></returns>
        public static double GetDirectDistance(double srcLon, double srcLat, double dstLon, double dstLat)
        {
            double radSrcLat = Deg2Rad(srcLat);
            double radSrcLon = Deg2Rad(srcLon);
            double radDstLat = Deg2Rad(dstLat);
            double radDstLon = Deg2Rad(dstLon);; 
            double a = radSrcLat - radDstLat;
            double b = radSrcLon - radDstLon;

            double directDistance = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) + Math.Cos(radSrcLat) * Math.Cos(radSrcLat) * Math.Cos(radDstLat) * Math.Pow(Math.Sin(b / 2), 2)));
            directDistance *= 6378137;
            directDistance = Math.Round(directDistance * 10000) / 10000;

            return directDistance;
        }

        /// <summary>
        /// 两经纬度方向角
        /// </summary>
        /// <param name="lon1"></param>
        /// <param name="lat1"></param>
        /// <param name="lon2"></param>
        /// <param name="lat2"></param>
        /// <returns></returns>
        public static double GetAzimuth(double lon1, double lat1, double lon2, double lat2)
        {
            lat1 = Deg2Rad(lat1);
            lat2 = Deg2Rad(lat2);
            lon1 = Deg2Rad(lon1);
            lon2 = Deg2Rad(lon2);

            double y = Math.Sin(lon2 - lon1) * Math.Cos(lat2);
            double x = Math.Cos(lat1) * Math.Sin(lat2) - Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(lon2 - lon1);
            double bearing = Math.Atan2(y, x);

            bearing = bearing * 180d / Math.PI;
            if (bearing < 0)
            {
                bearing += 360;
            }
            return bearing;
        }

        /// <summary>
        /// 将角度转为弧度
        /// </summary>
        /// <param name="degree"></param>
        /// <returns></returns>
        public static double Deg2Rad(double degree)
        {
            return degree * Math.PI / 180.0;
        }
    }
}
